#include "SocketClient.h"
#include "protocol.h"
//#include "PodServerInterface.h"

#define DEFAULT_SERVER_IP "137.222.103.92"
#define DEFAULT_SERVER_PORT 2006

int main(int argc, char* argv[])
{
   char *addr, buf[MAXMSG], *command, *parameters;
   int port;
   bool exit = false;
   
   if(argc > 1) {
      addr = argv[1];
   } else {
      addr = DEFAULT_SERVER_IP;
   }
   if(argc > 2) {
      port = atoi(argv[2]);
   } else {
      port = DEFAULT_SERVER_PORT;
   }
      
   SocketClient client(addr, tcp, port);
   //PodServerInterface psi;
      
   printf("Type \"help\" for help or \"quit\" to quit.\n");
   
   while (!exit)
   {
      memset(&buf, 0, MAXMSG + 1);
      std::cin.getline(buf, MAXMSG, '\n');
      command = strtok(buf, " ");
      parameters = strtok(NULL, "");
      if(strcmp(command, "move") == 0) {
          movedata mdata;
          sscanf(parameters, "%f %f %f %f %f %f",
             &mdata.x, &mdata.y, &mdata.z, &mdata.r, &mdata.p, &mdata.w);
          client.sendUnsignedInt(&client, cmdMOVE);
          client.sendStruct(&client, &mdata, sizeof(movedata));
      }
      else if(strcmp(command, "pollinputs") == 0) {
          polldata data;
          client.sendUnsignedInt(&client, cmdPOLLINPUTS);
          client.receiveStruct(&client, &data, sizeof(polldata));
          for(int i = 0; i < NUM_INPUTS; ++i) {
             printf("%f ", data.inputs[i]);
          }
          fprintf(stdout, "\n");
      } 
      else if(strcmp(command, "start") == 0) {
         client.sendUnsignedInt(&client, cmdSTART);
      }
      else if(strcmp(command, "stop") == 0) {
         client.sendUnsignedInt(&client, cmdSTOP);
      }
      else if(strcmp(command, "home") == 0) {
         client.sendUnsignedInt(&client, cmdHOME);
      }
      else if(strcmp(command, "park") == 0) {
         client.sendUnsignedInt(&client, cmdPARK);
      }
      else if(strcmp(command, "follow") == 0) {
         client.sendUnsignedInt(&client, cmdFOLLOW);
      }
      else if(strcmp(command, "getstate") == 0) {      
         client.sendUnsignedInt(&client, cmdGETSTATE);
         char state[MAXMSG];
         client.receiveString(&client, state);
      }
      else if(strcmp(command, "verbose") == 0) {
          client.sendUnsignedInt(&client, cmdVERBOSE);
      }
      else if(strcmp(command, "verbosepoll") == 0) {
          client.sendUnsignedInt(&client, cmdVERBOSEPOLL);
      }
      else if(strcmp(command, "verbosemove") == 0) {
          client.sendUnsignedInt(&client, cmdVERBOSEMOVE);
      }
      else if(strcmp(command, "quit") == 0) {
         exit = true;
      }
      else if(strcmp(command, "help") == 0) {
         printf("Pod Commands:\n");
         printf("move <x> <y> <z> <roll> <pitch> <yaw>\n");
         printf("pollinputs\n");
         printf("start\n");
         printf("stop\n");
         printf("home\n");
         printf("park\n");
         printf("follow\n");
         printf("getstate\n");
         printf("verbose\n");
         printf("verbosepoll\n");
         printf("verbosemove\n");
      }
   }
   
    /*movedata mdata;
    int i=0;
    psi.pollClient.sendUnsignedInt(&psi.pollClient, (unsigned int) cmdSTARTPOLLING);
    static clock_t rt_clock = clock();
    clock_t goal;*/
    //while(1)
    //{
        //psi.sendMove();
        //psi.pollInputs();
        /*goal = clock() + 15 - rt_clock;
        while(clock() < goal)
        {
            Sleep(1);
        }
        rt_clock = clock();*/
        
        /*for(int j=0; j<2; j++)
           printf("%i\t", psi.pdata.inputs[j]);
        printf("\n");*/
        
        /*
        mdata.x = (float)0+i;
        mdata.y = (float)1+i;
        mdata.z = (float)2+i;
        mdata.r = (float)3+i;
        mdata.p = (float)4+i;
        mdata.w = (float)5+i;
        psi.sendCommand(cmdMOVE);
        psi.sendMovedata(&mdata);
        printf("move sent: %f %f %f %f %f %f\n",mdata.x,mdata.y,mdata.z,mdata.r,mdata.p,mdata.w);
        Sleep(100);
        i++;
        */
    //}
    return 0;
}

